/**
 * @file /src/qnode.cpp
 *
 * @brief Ros communication central!
 *
 * @date 2021.7.21
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include <QDebug>
#include "../include/weeding_robot_gui/qnode.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace weeding_robot_gui {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
  init_argc(argc),
  init_argv(argv)
{}

QNode::~QNode() {
  if(ros::isStarted()) {
    ros::shutdown(); // explicitly needed since we use ros::start();
    ros::waitForShutdown();
  }
  wait();
}

bool QNode::init() {
  ros::init(init_argc,init_argv,"weeding_robot_gui");
  if ( ! ros::master::check() ) {
    return false;
  }
  ros::start(); // explicitly needed since our nodehandle is going out of scope.
  ros::NodeHandle n;
  // Add your ros communications here.
  chatter_publisher = n.advertise<std_msgs::String>("chatter", 1000);
  cmd_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);

  start();
  return true;
}

bool QNode::init(const std::string &master_url, const std::string &host_url) {
  std::map<std::string,std::string> remappings;
  remappings["__master"] = master_url;
  remappings["__hostname"] = host_url;
  ros::init(remappings,"weeding_robot_gui");
  if ( ! ros::master::check() ) {
    return false;
  }
  ros::start(); // explicitly needed since our nodehandle is going out of scope.
  ros::NodeHandle n;
  // Add your ros communications here.
  chatter_publisher = n.advertise<std_msgs::String>("chatter", 1000);
  cmd_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);

  start();
  return true;
}

void QNode::run() {
  ros::Rate loop_rate(30);  //如果图像传输卡顿，可能是ROS的loop_rate设置有问题，可以适当调高 current:3
  bool is_connected = true;
  while ( ros::ok() ) {
    if(is_connected)
    {
      ROS_INFO("Connected to the ros master");
      is_connected = false;
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
  std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
  Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}

/*****************************************************************************
** My Implementation [Functions]
*****************************************************************************/
//机器人控制指令发布
void QNode::move_base(char k,float speed_linear,float speed_trun)
{
  std::map<char, std::vector<float>> moveBindings
  {
    {'i', {1, 0, 0, 0}},
    {'o', {1, 0, 0, -1}},
    {'j', {0, 0, 0, 1}},
    {'l', {0, 0, 0, -1}},
    {'u', {1, 0, 0, 1}},
    {',', {-1, 0, 0, 0}},
    {'.', {-1, 0, 0, 1}},
    {'m', {-1, 0, 0, -1}},
    {'O', {1, -1, 0, 0}},
    {'I', {1, 0, 0, 0}},
    {'J', {0, 1, 0, 0}},
    {'L', {0, -1, 0, 0}},
    {'U', {1, 1, 0, 0}},
    {'<', {-1, 0, 0, 0}},
    {'>', {-1, -1, 0, 0}},
    {'M', {-1, 1, 0, 0}},
    {'t', {0, 0, 1, 0}},
    {'b', {0, 0, -1, 0}},
    {'k', {0, 0, 0, 0}},
    {'K', {0, 0, 0, 0}}
  };
  char key=k;
  //计算是往哪个方向
  float x = moveBindings[key][0];
  float y = moveBindings[key][1];
  float z = moveBindings[key][2];
  float th = moveBindings[key][3];
  //计算线速度和角速度
  float speed = speed_linear;
  float turn = speed_trun;
  // Update the Twist message
  geometry_msgs::Twist twist;
  twist.linear.x = x * speed;
  twist.linear.y = y * speed;
  twist.linear.z = z * speed;

  twist.angular.x = 0;
  twist.angular.y = 0;
  twist.angular.z = th * turn;

  // Publish it and resolve any remaining callbacks
  cmd_pub.publish(twist);
  ros::spinOnce();
}

void QNode::subImage(QString topic, int frame_id)
{
  ros::NodeHandle n;
  image_transport::ImageTransport it_(n);
  switch (frame_id) {
  case 1:
    image_sub0=it_.subscribe(topic.toStdString(),100,&QNode::subImageCallback_1,this);
    break;
  case 2:
    image_sub1=it_.subscribe(topic.toStdString(),100,&QNode::subImageCallback_2,this);
    break;
  case 3:
    image_sub2=it_.subscribe(topic.toStdString(),100,&QNode::subImageCallback_3,this);
    break;
  case 4:
    image_sub3=it_.subscribe(topic.toStdString(),100,&QNode::subImageCallback_4,this);
    break;
  }
  ros::spinOnce();
}

void QNode::subImageCallback_1(const sensor_msgs::ImageConstPtr& msg)
{
  cv_bridge::CvImagePtr cv_ptr;
  try
  {
    //深拷贝转换为opencv类型
    cv_ptr = cv_bridge::toCvCopy(msg,msg->encoding);
    QImage im=Mat2QImage(cv_ptr->image);
    Q_EMIT showImage(1,im);
  }
  catch (cv_bridge::Exception& e)
  {
    log(Error,("video frame1 exception: "+QString(e.what())).toStdString());
    return;
  }
}

void QNode::subImageCallback_2(const sensor_msgs::ImageConstPtr& msg)
{
  cv_bridge::CvImagePtr cv_ptr;
  try
  {
    //深拷贝转换为opencv类型
    cv_ptr = cv_bridge::toCvCopy(msg,msg->encoding);
    QImage im=Mat2QImage(cv_ptr->image);
    Q_EMIT showImage(2,im);
  }
  catch (cv_bridge::Exception& e)
  {
    log(Error,("video frame0 exception: "+QString(e.what())).toStdString());
    return;
  }
}

void QNode::subImageCallback_3(const sensor_msgs::ImageConstPtr& msg)
{
  cv_bridge::CvImagePtr cv_ptr;
  try
  {
    //深拷贝转换为opencv类型
    cv_ptr = cv_bridge::toCvCopy(msg,msg->encoding);
    QImage im=Mat2QImage(cv_ptr->image);
    Q_EMIT showImage(3,im);
  }
  catch (cv_bridge::Exception& e)
  {
    log(Error,("video frame1 exception: "+QString(e.what())).toStdString());
    return;
  }
}

void QNode::subImageCallback_4(const sensor_msgs::ImageConstPtr& msg)
{
  cv_bridge::CvImagePtr cv_ptr;
  try
  {
    //深拷贝转换为opencv类型
    cv_ptr = cv_bridge::toCvCopy(msg,msg->encoding);
    QImage im=Mat2QImage(cv_ptr->image);
    Q_EMIT showImage(4,im);
  }
  catch (cv_bridge::Exception& e)
  {
    log(Error,("video frame1 exception: "+QString(e.what())).toStdString());
    return;
  }
}

QImage QNode::Mat2QImage(cv::Mat const& src)
{
  QImage dest(src.cols, src.rows, QImage::Format_ARGB32);

  const float scale = 255.0;

  if (src.depth() == CV_8U) {
    if (src.channels() == 1) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          int level = src.at<quint8>(i, j);
          dest.setPixel(j, i, qRgb(level, level, level));
        }
      }
    } else if (src.channels() == 3) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          cv::Vec3b bgr = src.at<cv::Vec3b>(i, j);
          dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
        }
      }
    }
  } else if (src.depth() == CV_32F) {
    if (src.channels() == 1) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          int level = scale * src.at<float>(i, j);
          dest.setPixel(j, i, qRgb(level, level, level));
        }
      }
    } else if (src.channels() == 3) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          cv::Vec3f bgr = scale * src.at<cv::Vec3f>(i, j);
          dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
        }
      }
    }
  }
  return dest;
}


void QNode::log( const LogLevel &level, const std::string &msg) {
  logging_model.insertRows(logging_model.rowCount(),1);
  std::stringstream logging_model_msg;
  switch ( level ) {
  case(Debug) : {
    ROS_DEBUG_STREAM(msg);
    logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
    break;
  }
  case(Info) : {
    ROS_INFO_STREAM(msg);
    logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
    break;
  }
  case(Warn) : {
    ROS_WARN_STREAM(msg);
    logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
    break;
  }
  case(Error) : {
    ROS_ERROR_STREAM(msg);
    logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
    break;
  }
  case(Fatal) : {
    ROS_FATAL_STREAM(msg);
    logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
    break;
  }
  }
  QVariant new_row(QString(logging_model_msg.str().c_str()));
  logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
  Q_EMIT loggingUpdated(); // used to readjust the scrollbar
}

}  // namespace weeding_robot_gui
